/*
 * Robot.java
 *
 * Created on November 7, 2007, 12:04 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package edu.ncsu.robowulf;

import com.sun.spot.peripheral.Spot;
import com.sun.spot.resourcesharing.ResourceUnavailableException;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IOutputPin;
import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.sensorboard.peripheral.LIS3L02AQAccelerometer;
import com.sun.spot.sensorboard.peripheral.Servo;
import com.sun.spot.util.Utils;
import com.sun.squawk.util.IntHashtable;
import edu.ncsu.external_sensors.IRobotSensor;
import edu.ncsu.external_sensors.MaxSonarEZ1UltrasoundSensor;
import edu.ncsu.external_sensors.PINGUltrasoundSensor;
import edu.ncsu.external_sensors.UltrasoundSensor;

/**
 * Implementation of IRobotController for the Robowulf robot platform
 * @author Douglas McClusky
 */
public class Robowulf implements IRobotController {
    public static final int MAIN_ULTRASOUND = EDemoBoard.D0;
    
    public static final int METERS = 0;
    public static final int CENTIMETERS = 1;
    public static final int MILLIMETERS = 2;
    public static final int INCHES = 3;
    private static final double[] UNITS_PER_METER = {1, 100, 1000, 39.3700787401575};
    private int units = 0;
    
    private int[] stop = new int[2];
    private int[] halfwidths = new int[2];
    private IOutputPin[] hPins = EDemoBoard.getInstance().getOutputPins();
    private RobowulfServo[] wheels = {new RobowulfServo(hPins[0], true), new RobowulfServo(hPins[1], false)};
    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();
    private IScalarInput[] adcs = EDemoBoard.getInstance().getScalarInputs();
    private LIS3L02AQAccelerometer acc = (LIS3L02AQAccelerometer)EDemoBoard.getInstance().getAccelerometer();
    
    private static Robowulf mrSpiffy = new Robowulf();
    private boolean servoActive = false;
    
    private int [] conv;  // duration conversions for movements
    private PINGUltrasoundSensor ping = new PINGUltrasoundSensor(EDemoBoard.getInstance().getIOPins()[MAIN_ULTRASOUND],UltrasoundSensor.INCHES);
    private MaxSonarEZ1UltrasoundSensor maxSonar = new MaxSonarEZ1UltrasoundSensor(EDemoBoard.getInstance().getIOPins()[MAIN_ULTRASOUND], UltrasoundSensor.INCHES);
    
    public static final int FORWARD_ULTRASOUND_SENSOR = 0;
    private int sensorIndex = 1;
    private IntHashtable sensors = new IntHashtable();
    
    /**
     * Register an IRobotSensor with Robowulf. <br>
     * A forward-facing ultrasound sensor is included with the base platform,
     * but other sensors can be interfaced through the power board and registered to Robowulf
     * via this method. These sensors can later be accessed via senseFrom(<index>), where <index> is a counter
     * that starts at 1 and increments and new sensors are added.<br>
     * <br>
     * Remember to include an appropriate resister in the signal connection from the sensor to the board.
     * @param sensor - the IRobobotSensor 
     */
    public void addSensor(IRobotSensor sensor) {
        sensors.put(sensorIndex, sensor);
        sensorIndex++;
    }
    
    /**
     * Since multiple competing IRobotControllers would be confusing, Robowulf must be accessed from its static instance.
     */
    private Robowulf() {
        acc.setScale(LIS3L02AQAccelerometer.SCALE_2G);        // start using 2G scale
        //adcs[1].addIScalarInputThresholdListener(new BatteryPackThresholdListener(adcs[1]));
        int def[] = {1454, 1454, 600, 600, 597/4, 145, 550, 550, INCHES}; // Default configuration
        setCalibration(def);
        resetWheelCalibration();
        Spot.getInstance().getDriverRegistry().add(this);
    }
    
    private synchronized void resetWheelCalibration() {
        for ( int i=0; i<2; i++) {
             wheels[i].setBounds(stop[i]-halfwidths[i],stop[i]+halfwidths[i]);
        }
    }
    
    /**
     * Set the servo calibration array for this Robowulf
     * @param cal - a 9-int-array whose values are:
     * <ol>
     * <li>The stop pulse width for the left servo
     * <li>The stop pulse width for the right servo
     * <li>The amount from stop to go in the positive pulse-width direction for movement
     * <li>The amount from stop to go in the negative pulse-width direction for movement
     * <li>Units / sec for the forward direction
     * <li>Units / sec for the backward direction
     * <li>Degrees / sec for the left direction
     * <li>Degrees / sec for the right direction
     * <li>Unit of length used for forward and backwards calibration numbers
     * </ol>
     */
    public synchronized void setCalibration(int cal[]) {
        stop[0] = cal[0];
        stop[1] = cal[1];
        halfwidths[0] = cal[2];
        halfwidths[1] = cal[3];
        int convtemp[] = {(int)Math.floor(cal[4] * UNITS_PER_METER[cal[8]]), (int)Math.floor(cal[5] * UNITS_PER_METER[cal[8]]), cal[6], cal[7]};
        conv = convtemp;
        for (int i=0; i<4; i++) {
            //System.out.println(conv[i]);
        }
        resetWheelCalibration();
    }
    
    /**
     * @return the Singleton instance of Robowulf.
     */
    public static IRobotController getInstance() {
        return mrSpiffy;
    }
            
    public void stop() {
        brake(250);
        for(int i=0; i<2; i++) {  //set pulses to 0
            wheels[i].setValue(0);
        }
    }
    
    public synchronized void forward (double length) {
        //System.out.println("forward");
        servoActive = true;
        //set pulse widths
        wheels[0].setPosition(0); 
        wheels[1].setPosition(1);
        //System.out.println("sleeping for: " + length*conv[0]/UNITS_PER_METER[units]);
        Utils.sleep((long)Math.floor(length*conv[0]/UNITS_PER_METER[units]));
        servoActive = false;
    }
    
    public synchronized void backward (double length) {
        //System.out.println("back");
        servoActive = true;
        //set pulse widths
        wheels[0].setPosition(1); 
        wheels[1].setPosition(0);
        //System.out.println("sleeping for: " + length*conv[1]/UNITS_PER_METER[units]);
        Utils.sleep((long)Math.floor(length*conv[1]/UNITS_PER_METER[units]));
        servoActive = false;
    }
    
    public synchronized void right (double degrees) {
        //System.out.println("right");
        servoActive = true;
        //set pulse widths
        wheels[0].setPosition(0);
        wheels[1].setPosition(0);
        Utils.sleep((long)Math.floor(degrees*conv[3]/90));
        servoActive = false;
    }
    
    public synchronized void left (double degrees) {
        //System.out.println("left");
        servoActive = true;
        //set pulse widths
        wheels[0].setPosition(1);
        wheels[1].setPosition(1);
        Utils.sleep((long)Math.floor(degrees * conv[2]/90));
        servoActive = false;
    }
    
    public synchronized void brake(long dur) {
        servoActive = true;
        //set pulse widths
        for (int i = 0; i<2; i++) {
            wheels[i].setPosition((float).5);
        }
        Utils.sleep(dur);
        servoActive = false;
    }
    
    public String getDriverName() {
        return "Robowulf";
    }

    public boolean tearDown() {
        return !servoActive;
    }

    public void setUp() {
    }

    public void shutDown() {
    }
    
    public double senseForwardRange() throws ResourceUnavailableException {
        if (maxSonar.testConnection()) {
            return maxSonar.getConvertedValue();
        }
        if (ping.testConnection()) {
            return ping.getConvertedValue();
        }
        throw new ResourceUnavailableException("No supported UltrasoundSensor detected.");
    }
    
    /**
     * Returns the convereted sensor value of the indicated IRobotSensor.
     * @throws ResourceUnavailableException if the indicated IRobotSensor has not been registered to Robowulf, or
     * if it cannot be detected.
     */
    public synchronized double senseFrom(int index) throws ResourceUnavailableException {
        if (index == FORWARD_ULTRASOUND_SENSOR) {
            return senseForwardRange();
        }
        if (index < this.sensorIndex) {
            return ((IRobotSensor)sensors.get(index)).getConvertedValue();
        }
        throw new ResourceUnavailableException("Requested IRobotSensor not registered with this Robowulf.");
    }

    public void setUnits(int units) {
        this.units = units;
    }
    
}
